| Apm飞控学习笔记之添加我的飞行模式 | 您所在的位置：网站首页 › apm master mode设置 › Apm飞控学习笔记之添加我的飞行模式 |
Apm飞控学习笔记之添加我的飞行模式
|
目录
PX4/APM/飞控的学习笔记前言-Cxm_chen_taifu的博客-CSDN博客开始了 开始了终于有时间可以学习飞控了此文章是用来当目录,我会持续更新我的学习之旅,希望能对各位有所帮助如果有错误的地方还请各位大佬不吝赐教,可以在评论区回复相关问题来交流。此帖持续更新... 这次的章节记录一下是如何实现在APM飞控中添加自己的模式 这里还是用的4.07版本的飞控固件 由于网上的很多版本都是3.6版本固件的添加模式 但是在最新的版本上 APM的模式架构发生了很大的变化所以这个笔记只适用于新版本的固件 先放官方的链接 APM官方模式添加说明 架构
官方建议直接去复制一个他原本的模式去修改 更加方便 ArduCopter\mode.h //这里包含了所有模式 一共26种 class Mode { public: // Auto Pilot Modes enumeration enum class Number : uint8_t { STABILIZE = 0, // manual airframe angle with manual throttle ACRO = 1, // manual body-frame angular rate with manual throttle ALT_HOLD = 2, // manual airframe angle with automatic throttle AUTO = 3, // fully automatic waypoint control using mission commands GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands LOITER = 5, // automatic horizontal acceleration with automatic throttle RTL = 6, // automatic return to launching point CIRCLE = 7, // automatic circular flight with automatic throttle LAND = 9, // automatic landing with horizontal position control DRIFT = 11, // semi-automous position, yaw and throttle control SPORT = 13, // manual earth-frame angular rate control with manual throttle FLIP = 14, // automatically flip the vehicle on the roll axis AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains POSHOLD = 16, // automatic position hold with manual override, with automatic throttle BRAKE = 17, // full-brake using inertial/GPS system, no pilot input THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder FOLLOW = 23, // follow attempts to follow another vehicle or ground station ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers AUTOROTATE = 26, // Autonomous autorotation };ArduCopter\mode_stabilize.cpp 我们先参考一下官方推荐的模式 #include "Copter.h" /* * Init and run calls for stabilize flight mode */ // stabilize_run - runs the main stabilize controller // should be called at 100hz or more void ModeStabilize::run() { // apply simple mode transform to pilot inputs update_simple_mode(); // convert pilot input to lean angles float target_roll, target_pitch; get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // get pilot's desired yaw rate float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!motors->armed()) { // Motors should be Stopped motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); } else if (copter.ap.throttle_zero) { // Attempting to Land motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } switch (motors->get_spool_state()) { case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); break; case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms_smoothly(); break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: // clear landing flag above zero throttle if (!motors->limit.throttle_lower) { set_land_complete(false); } break; case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_DOWN: // do nothing break; } // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // output pilot's throttle attitude_control->set_throttle_out(get_pilot_desired_throttle(), true, g.throttle_filt); }直接通过VS code 跳转到 他的ModeStabilize 类里 回到了 ArduCopter\mode.h 中 class ModeStabilize : public Mode { public: // inherit constructor using Mode::Mode; virtual void run() override; bool requires_GPS() const override { return false; } bool has_manual_throttle() const override { return true; } bool allows_arming(bool from_gcs) const override { return true; }; bool is_autopilot() const override { return false; } protected: const char *name() const override { return "STABILIZE"; } const char *name4() const override { return "STAB"; } private: };我们大致就知道了 整个模式的内容是再那里 需要在那里定义声明 我们直接发扬传统开始复制粘贴 然后修改一下 ArduCopter\mode.h 新建我们的模式类 并继承父类 这里我们直接照搬stabilize模式 ++ //newfly class ModeNewFly : public Mode { public: // inherit constructor using Mode::Mode; virtual void run() override; bool requires_GPS() const override { return false; } bool has_manual_throttle() const override { return true; } bool allows_arming(bool from_gcs) const override { return true; }; bool is_autopilot() const override { return false; } protected: const char *name() const override { return "NewFly"; } const char *name4() const override { return "NewFly"; } private: }; ///++内容不用改 因为使用了多态我们只需要修改名字就可以 最后再添加自己的功能 ArduCopter\mode_newfly.cpp 通过架构图我们可以知道 run就是存放模式功能的地方 并且是400Hz的速度执行 我们在这里 给一个地面站消息输出用于验证 让每次波动到这个模式的时候就发送 当前模式 mode fly /* 2022/2/24 @CHENxiaomingming e-mail:[email protected] newfly */ #include "Copter.h" #include //地面站 void ModeNewFly::run() { // apply simple mode transform to pilot inputs update_simple_mode(); gcs().send_text(MAV_SEVERITY_CRITICAL, //地面站消息发送 "mode fly"); }到这里我们内容基本就添加完毕了 但是还没有真的添加进去 我们先看官方文档 ArduCopter\mode.h 添加进模式里面 先将我们的模式名字 NEWFLY 和编号 写进去 class Mode { public: // Auto Pilot Modes enumeration enum class Number : uint8_t { STABILIZE = 0, // manual airframe angle with manual throttle ACRO = 1, // manual body-frame angular rate with manual throttle ALT_HOLD = 2, // manual airframe angle with automatic throttle AUTO = 3, // fully automatic waypoint control using mission commands GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands LOITER = 5, // automatic horizontal acceleration with automatic throttle RTL = 6, // automatic return to launching point CIRCLE = 7, // automatic circular flight with automatic throttle LAND = 9, // automatic landing with horizontal position control DRIFT = 11, // semi-automous position, yaw and throttle control SPORT = 13, // manual earth-frame angular rate control with manual throttle FLIP = 14, // automatically flip the vehicle on the roll axis AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains POSHOLD = 16, // automatic position hold with manual override, with automatic throttle BRAKE = 17, // full-brake using inertial/GPS system, no pilot input THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder FOLLOW = 23, // follow attempts to follow another vehicle or ground station ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers AUTOROTATE = 26, // Autonomous autorotation NEWFLY = 27, //+ };ArduCopter\Copter.h 搜索ModeAcro 照样子写一个就可以 Mode *flightmode; #if MODE_ACRO_ENABLED == ENABLED #if FRAME_CONFIG == HELI_FRAME ModeAcro_Heli mode_acro; #else ModeAcro mode_acro; #endif #endif ModeAltHold mode_althold; #if MODE_AUTO_ENABLED == ENABLED ModeAuto mode_auto; #endif #if AUTOTUNE_ENABLED == ENABLED AutoTune autotune; ModeAutoTune mode_autotune; #endif #if MODE_BRAKE_ENABLED == ENABLED ModeBrake mode_brake; #endif #if MODE_CIRCLE_ENABLED == ENABLED ModeCircle mode_circle; #endif #if MODE_DRIFT_ENABLED == ENABLED ModeDrift mode_drift; #endif #if MODE_FLIP_ENABLED == ENABLED ModeFlip mode_flip; #endif #if MODE_FOLLOW_ENABLED == ENABLED ModeFollow mode_follow; #endif #if MODE_GUIDED_ENABLED == ENABLED ModeGuided mode_guided; #endif ModeLand mode_land; #if MODE_LOITER_ENABLED == ENABLED ModeLoiter mode_loiter; #endif #if MODE_POSHOLD_ENABLED == ENABLED ModePosHold mode_poshold; #endif #if MODE_RTL_ENABLED == ENABLED ModeRTL mode_rtl; #endif #if FRAME_CONFIG == HELI_FRAME ModeStabilize_Heli mode_stabilize; #else ModeStabilize mode_stabilize; #endif #if MODE_SPORT_ENABLED == ENABLED ModeSport mode_sport; #endif #if MODE_SYSTEMID_ENABLED == ENABLED ModeSystemId mode_systemid; #endif #if ADSB_ENABLED == ENABLED ModeAvoidADSB mode_avoid_adsb; #endif #if MODE_THROW_ENABLED == ENABLED ModeThrow mode_throw; #endif #if MODE_GUIDED_NOGPS_ENABLED == ENABLED ModeGuidedNoGPS mode_guided_nogps; #endif #if MODE_SMARTRTL_ENABLED == ENABLED ModeSmartRTL mode_smartrtl; #endif #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED ModeFlowHold mode_flowhold; #endif #if MODE_ZIGZAG_ENABLED == ENABLED ModeZigZag mode_zigzag; #endif #if MODE_AUTOROTATE_ENABLED == ENABLED ModeAutorotate mode_autorotate; #endif //+ #if MODE_NEWFLY_ENABLED == ENABLED ModeNewFly mode_newfly; #endifArduCopter\config.h 需要声明一下 // // Position Hold - enable holding of global position #ifndef MODE_POSHOLD_ENABLED # define MODE_POSHOLD_ENABLED ENABLED #endif // // RTL - Return To Launch #ifndef MODE_RTL_ENABLED # define MODE_RTL_ENABLED ENABLED #endif // // SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home #ifndef MODE_SMARTRTL_ENABLED # define MODE_SMARTRTL_ENABLED ENABLED #endif // // Sport - fly vehicle in rate-controlled (earth-frame) mode #ifndef MODE_SPORT_ENABLED # define MODE_SPORT_ENABLED !HAL_MINIMIZE_FEATURES #endif // // System ID - conduct system identification tests on vehicle #ifndef MODE_SYSTEMID_ENABLED # define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES #endif // // Throw - fly vehicle after throwing it in the air #ifndef MODE_THROW_ENABLED # define MODE_THROW_ENABLED ENABLED #endif // // ZigZag - allow vehicle to fly in a zigzag manner with predefined point A B #ifndef MODE_ZIGZAG_ENABLED # define MODE_ZIGZAG_ENABLED !HAL_MINIMIZE_FEATURES #endif //+new fly #ifndef MODE_NEWFLY_ENABLED # define MODE_NEWFLY_ENABLED ENABLED #endifArduCopter\mode.cpp 需要在里面添加编号和建立映射 mode_from_mode_num()里 Mode *Copter::mode_from_mode_num(const Mode::Number mode) { Mode *ret = nullptr; switch (mode) { #if MODE_ACRO_ENABLED == ENABLED case Mode::Number::ACRO: ret = &mode_acro; break; #endif case Mode::Number::STABILIZE: ret = &mode_stabilize; break; case Mode::Number::ALT_HOLD: ret = &mode_althold; break; #if MODE_AUTO_ENABLED == ENABLED case Mode::Number::AUTO: ret = &mode_auto; break; #endif #if MODE_CIRCLE_ENABLED == ENABLED case Mode::Number::CIRCLE: ret = &mode_circle; break; #endif #if MODE_LOITER_ENABLED == ENABLED case Mode::Number::LOITER: ret = &mode_loiter; break; #endif #if MODE_GUIDED_ENABLED == ENABLED case Mode::Number::GUIDED: ret = &mode_guided; break; #endif case Mode::Number::LAND: ret = &mode_land; break; #if MODE_RTL_ENABLED == ENABLED case Mode::Number::RTL: ret = &mode_rtl; break; #endif #if MODE_DRIFT_ENABLED == ENABLED case Mode::Number::DRIFT: ret = &mode_drift; break; #endif #if MODE_SPORT_ENABLED == ENABLED case Mode::Number::SPORT: ret = &mode_sport; break; #endif #if MODE_FLIP_ENABLED == ENABLED case Mode::Number::FLIP: ret = &mode_flip; break; #endif #if AUTOTUNE_ENABLED == ENABLED case Mode::Number::AUTOTUNE: ret = &mode_autotune; break; #endif #if MODE_POSHOLD_ENABLED == ENABLED case Mode::Number::POSHOLD: ret = &mode_poshold; break; #endif #if MODE_BRAKE_ENABLED == ENABLED case Mode::Number::BRAKE: ret = &mode_brake; break; #endif #if MODE_THROW_ENABLED == ENABLED case Mode::Number::THROW: ret = &mode_throw; break; #endif #if ADSB_ENABLED == ENABLED case Mode::Number::AVOID_ADSB: ret = &mode_avoid_adsb; break; #endif #if MODE_GUIDED_NOGPS_ENABLED == ENABLED case Mode::Number::GUIDED_NOGPS: ret = &mode_guided_nogps; break; #endif #if MODE_SMARTRTL_ENABLED == ENABLED case Mode::Number::SMART_RTL: ret = &mode_smartrtl; break; #endif #if OPTFLOW == ENABLED case Mode::Number::FLOWHOLD: ret = (Mode *)g2.mode_flowhold_ptr; break; #endif #if MODE_FOLLOW_ENABLED == ENABLED case Mode::Number::FOLLOW: ret = &mode_follow; break; #endif #if MODE_ZIGZAG_ENABLED == ENABLED case Mode::Number::ZIGZAG: ret = &mode_zigzag; break; #endif #if MODE_SYSTEMID_ENABLED == ENABLED case Mode::Number::SYSTEMID: ret = (Mode *)g2.mode_systemid_ptr; break; #endif ///NEWFLY + #if MODE_NEWFLY_ENABLED == ENABLED case Mode::Number::NEWFLY: ret = &mode_newfly; break; #endif #if MODE_AUTOROTATE_ENABLED == ENABLED case Mode::Number::AUTOROTATE: ret = &mode_autorotate; break; #endif default: break; } return ret; }到这里其实就可以直接编译使用了 但是如果想让地面站有显示的化 还需要一些操作 modules\mavlink\message_definitions\v1.0\ardupilotmega.xml 在这里文件里面 搜索COPTER_MODE 添加进这个枚举里面就可以 A mapping of copter flight modes for custom_mode field of heartbeat.到这里 官方的说明就结束了 现在我们编译 然后下载到飞控中
但是地面站并没有显示我们的模式 官方说有的地面站会填充 可能是版本问题 所以我们只能通过修改参数 来开启这个模式 在官方文档里面有说明 ArduCopter\Parameters.cpp 这里面搜索FLTMODE1 这里面是和地面上(上面的图)对应的六个模式的初始化模式 // @Param: FLTMODE1 // @DisplayName: Flight Mode 1 // @Description: Flight mode when Channel 5 pwm is 1230, 1360, 1490, 1620, =1750 // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6),刚好6个 直接跳转到 FLIGHT_MODE_4配置里面去 ArduCopter\config.h 下面的 STABILZE 就是默认模式 如果想吧默认模式修改 直接改成 NEWFLY 就可以了 但是我不推荐这样的操作 因为地面站有参数配置 #ifndef FLIGHT_MODE_1 # define FLIGHT_MODE_1 Mode::Number::STABILIZE #endif #ifndef FLIGHT_MODE_2 # define FLIGHT_MODE_2 Mode::Number::STABILIZE #endif #ifndef FLIGHT_MODE_3 # define FLIGHT_MODE_3 Mode::Number::STABILIZE #endif #ifndef FLIGHT_MODE_4 # define FLIGHT_MODE_4 Mode::Number::STABILIZE #endif #ifndef FLIGHT_MODE_5 # define FLIGHT_MODE_5 Mode::Number::STABILIZE #endif #ifndef FLIGHT_MODE_6 # define FLIGHT_MODE_6 Mode::Number::STABILIZE #endif这里我们直接通过地面站的参数配置来设置我们的模式
切换到模式4 成功输出
模式之间可以顺利切换
END |
https://ardupilot.org/dev/docs/apmcopter-adding-a-new-flight-mode.html
在全部设置里面 搜索MODE 里面就有上面的参数了 后面0-26就是可选模式 因为我们的模式是27 就给27就可以了 然后写入 
【本文地址】
公司简介
联系我们